/*
 * SimpleCDMOTOR Arduino Library
 * Stepper Motor Control via CDBUS/CDNET
 * 
 * Copyright (c) 2024
 * Licensed under MIT License
 * Based on cdstep motor control implementation
 */

#include "SimpleCDMOTOR.h"

SimpleCDMOTOR::SimpleCDMOTOR() {
    _motor_port = 0x1001;  // Default motor control port
    _motor_addr = 0x01;    // Default motor address
    _update_interval = 1000; // 1ms update interval
    _last_update = 0;
    
    // Default configuration
    _config.enable_pin = 8;
    _config.dir_pin = 9;
    _config.step_pin = 10;
    _config.ms1_pin = 11;
    _config.ms2_pin = 12;
    _config.ms3_pin = 13;
    _config.home_pin = 2;
    _config.limit_pin = 3;
    
    _config.steps_per_rev = 200;
    _config.max_speed = 1000;
    _config.max_accel = 5000;
    _config.current_limit = 1000;
    
    _config.kp = 1.0;
    _config.ki = 0.1;
    _config.kd = 0.01;
    
    // Initialize state
    memset(&_state, 0, sizeof(motor_state_t));
    _state.state = MOTOR_STATE_DISABLED;
    _state.control_mode = CONTROL_MODE_POSITION;
}

void SimpleCDMOTOR::begin(HardwareSerial *serial, uint8_t address, uint16_t motor_port) {
    _cdnet.begin(serial, address);
    _motor_port = motor_port;
    _motor_addr = address;
    
    initializeMotor();
}

void SimpleCDMOTOR::configure(const motor_config_t &config) {
    _config = config;
    initializeMotor();
}

void SimpleCDMOTOR::initializeMotor() {
    // Configure pins
    pinMode(_config.enable_pin, OUTPUT);
    pinMode(_config.dir_pin, OUTPUT);
    pinMode(_config.step_pin, OUTPUT);
    pinMode(_config.ms1_pin, OUTPUT);
    pinMode(_config.ms2_pin, OUTPUT);
    pinMode(_config.ms3_pin, OUTPUT);
    pinMode(_config.home_pin, INPUT_PULLUP);
    pinMode(_config.limit_pin, INPUT_PULLUP);
    
    // Set microstepping (1/16 by default)
    digitalWrite(_config.ms1_pin, HIGH);
    digitalWrite(_config.ms2_pin, HIGH);
    digitalWrite(_config.ms3_pin, HIGH);
    
    // Disable motor initially
    disable();
}

void SimpleCDMOTOR::enable() {
    digitalWrite(_config.enable_pin, LOW);  // Active low
    _state.state = MOTOR_STATE_ENABLED;
}

void SimpleCDMOTOR::disable() {
    digitalWrite(_config.enable_pin, HIGH); // Active low
    _state.state = MOTOR_STATE_DISABLED;
    _state.current_speed = 0;
}

void SimpleCDMOTOR::setPosition(int32_t position) {
    _state.target_pos = position;
    _state.control_mode = CONTROL_MODE_POSITION;
    if (_state.state == MOTOR_STATE_ENABLED) {
        _state.state = MOTOR_STATE_MOVING;
    }
}

void SimpleCDMOTOR::setVelocity(int32_t velocity) {
    _state.target_speed = constrain(velocity, -_config.max_speed, _config.max_speed);
    _state.control_mode = CONTROL_MODE_VELOCITY;
    if (_state.state == MOTOR_STATE_ENABLED) {
        _state.state = MOTOR_STATE_MOVING;
    }
}

void SimpleCDMOTOR::home() {
    _state.state = MOTOR_STATE_HOMING;
    _state.target_pos = -1000000; // Move towards home
}

void SimpleCDMOTOR::stop() {
    _state.target_speed = 0;
    _state.target_pos = _state.current_pos;
    _state.state = MOTOR_STATE_ENABLED;
}

bool SimpleCDMOTOR::isEnabled() {
    return _state.state != MOTOR_STATE_DISABLED;
}

bool SimpleCDMOTOR::isMoving() {
    return _state.state == MOTOR_STATE_MOVING || _state.state == MOTOR_STATE_HOMING;
}

bool SimpleCDMOTOR::isHomed() {
    return _state.homed;
}

int32_t SimpleCDMOTOR::getPosition() {
    return _state.current_pos;
}

int32_t SimpleCDMOTOR::getTargetPosition() {
    return _state.target_pos;
}

uint8_t SimpleCDMOTOR::getState() {
    return _state.state;
}

void SimpleCDMOTOR::setMaxSpeed(uint16_t max_speed) {
    _config.max_speed = max_speed;
}

void SimpleCDMOTOR::setMaxAccel(uint16_t max_accel) {
    _config.max_accel = max_accel;
}

void SimpleCDMOTOR::setPID(float kp, float ki, float kd) {
    _config.kp = kp;
    _config.ki = ki;
    _config.kd = kd;
}

void SimpleCDMOTOR::update() {
    uint32_t current_time = millis();
    if (current_time - _last_update < (_update_interval / 1000)) {
        return;
    }
    _last_update = current_time;
    
    _cdnet.update();
    
    updateLimitSwitches();
    updateTrajectory();
    updatePID();
    
    // Process incoming CDNET packets
    uint8_t src, buffer[64];
    uint16_t port;
    uint8_t len = _cdnet.readL1(&src, &port, buffer, sizeof(buffer));
    if (len > 0 && port == _motor_port) {
        processMotorPacket(buffer, len);
    }
}

void SimpleCDMOTOR::updateLimitSwitches() {
    bool home_triggered = digitalRead(_config.home_pin) == LOW;
    bool limit_triggered = digitalRead(_config.limit_pin) == LOW;
    
    if (home_triggered && _state.state == MOTOR_STATE_HOMING) {
        _state.current_pos = 0;
        _state.target_pos = 0;
        _state.homed = true;
        _state.state = MOTOR_STATE_ENABLED;
    }
    
    _state.limit_reached = limit_triggered;
}

void SimpleCDMOTOR::updateTrajectory() {
    if (_state.control_mode == CONTROL_MODE_POSITION) {
        int32_t error = _state.target_pos - _state.current_pos;
        
        if (abs(error) < 2) { // Close enough
            _state.current_speed = 0;
            if (_state.state == MOTOR_STATE_MOVING) {
                _state.state = MOTOR_STATE_ENABLED;
            }
            return;
        }
        
        // Simple trajectory planning
        int32_t max_steps = _config.max_speed / 1000; // per ms
        int32_t steps = constrain(error, -max_steps, max_steps);
        _state.tc_pos = _state.current_pos + steps;
    } else if (_state.control_mode == CONTROL_MODE_VELOCITY) {
        _state.tc_pos += _state.target_speed / 1000; // per ms
    }
}

void SimpleCDMOTOR::updatePID() {
    if (_state.state == MOTOR_STATE_DISABLED) {
        return;
    }
    
    float error = _state.tc_pos - _state.current_pos;
    
    // PID calculation
    _state.pid_integral += error * (_config.ki / 1000.0);
    _state.pid_integral = constrain(_state.pid_integral, -1000, 1000);
    
    float derivative = (error - _state.pid_last_error) * (_config.kd * 1000.0);
    _state.pid_last_error = error;
    
    _state.pid_output = (_config.kp * error) + _state.pid_integral + derivative;
    
    // Convert to steps
    int32_t steps = (int32_t)_state.pid_output;
    if (abs(steps) > 0) {
        sendStepCommand(steps);
        _state.current_pos += steps;
    }
}

void SimpleCDMOTOR::sendStepCommand(int32_t steps) {
    if (steps == 0) return;
    
    digitalWrite(_config.dir_pin, steps > 0 ? HIGH : LOW);
    
    int32_t abs_steps = abs(steps);
    for (int32_t i = 0; i < abs_steps; i++) {
        digitalWrite(_config.step_pin, HIGH);
        delayMicroseconds(2);
        digitalWrite(_config.step_pin, LOW);
        delayMicroseconds(2);
    }
}

void SimpleCDMOTOR::sendMotorCommand(uint8_t command, int32_t value) {
    uint8_t data[5];
    data[0] = command;
    memcpy(&data[1], &value, 4);
    _cdnet.sendL1(_motor_addr, _motor_port, data, sizeof(data));
}

void SimpleCDMOTOR::processMotorPacket(uint8_t *data, uint8_t len) {
    if (len < 1) return;
    
    uint8_t command = data[0];
    int32_t value = 0;
    
    if (len >= 5) {
        memcpy(&value, &data[1], 4);
    }
    
    switch (command) {
        case 0x01: // Set position
            setPosition(value);
            break;
        case 0x02: // Set velocity
            setVelocity(value);
            break;
        case 0x03: // Enable motor
            enable();
            break;
        case 0x04: // Disable motor
            disable();
            break;
        case 0x05: // Home motor
            home();
            break;
        case 0x06: // Stop motor
            stop();
            break;
        case 0x10: // Get position response
            _state.current_pos = value;
            break;
    }
}